#!/usr/bin/env python

import roslib; roslib.load_manifest('team4')
import rospy
from lift_test import Tray 
from geometry_msgs.msg import PointStamped
from ObjectUnstacker import *
from ObjectStacker import *
from PlatePicker import *
from Room3 import *
from free_table_spot import *
from pr2_python import base, head, gripper, world_interface
#from filename_of_your_module import * <- does not need the .py. Should import classes, functions, and maybe global vars

# Before calling import on your file, make sure of three things:
   # if it is team4/ then it can stay in scripts/ or be in src/
   # if it is in stacked_objects/ it has to be in src/
   # there cannot be an init_node in your code. If your code needs that to run, then you need an interface of some sort and we need to launch it seperately

# If there is anything that your code needs launched in addition to the usual, add it to the demo.launch file.

# Really, this should be done as a state machine. Just pretend it's one for now.

if __name__ == "__main__":
   rospy.init_node("team4_demo")
   
   rospy.loginfo("Beginning team 4 demo")
   rospy.loginfo("Creating all class instances")
   # instantiate classes here (if they need instantiating)
   tray = Tray()
   base_mover=base.Base()
   stacker = ObjectStacker()
   picker = PlatePicker()
   room3 = Room3()
   ou = ObjectUnstacker()
   gr_r = gripper.Gripper('right_arm')
   gr_r.open()
   gr_l = gripper.Gripper('left_arm')
   gr_l.open()
   ou.reset_everything()
   stacker.move_arms_back()
   h=head.Head()
   h.look_at_relative_point(0.5, 0.0, 0.75)
   ou.reset_everything()
   rospy.loginfo("Going to dirty table to pick up tray. ")
   # Go to dirty table
   print "Press key when you're ready to move"
   #raw_input()
   #room3.goto_dirty_table()
   print "Press key when you're ready to pickup tray"
   #raw_input()

   tray.pick_up_tray()
   
   rospy.loginfo("Going back to clean table")
   # go to clean table
   print "Press key when you're ready to move"
   raw_input()
   #b.move_to(-0.6, 0.2, -1.51)
   
   b = base.Base()
   currentPose = b.get_current_pose()
   b.move_to(currentPose.x, currentPose.y, currentPose.z+1.51)
   h = head.Head()
   h.look_at_relative_point(0.0, -0.8, 0.8)
   
   #room3.move_to_table()
   
   rospy.loginfo("Scanning table to find all objects and free space")
   print "Press key to move tray down"
   raw_input()
#   tray.get_tray_out_of_the_way()
   
   # scan area
   center_free_spot=get_free_spot(width=0.4,length=0.6)
   h.look_at_relative_point(0.8, 0.0, 0.8)
   
   base.move_manipulable_pose(center_free_spot.pose.position.x,center_free_spot.pose.position.y,center_free_spot.pose.position.z)
   
   # move to robot in front of freespace on table
   rospy.loginfo("Putting tray down")
   print "Press key to move tray down"
   raw_input()
   tray.put_down_tray(center_free_spot)
#   tmp=center_free_spot
#   tmp.pose.position.z+=0.3
#   if not tray.moveTray2Pose(tmp)==False:
#      tray.put_down()
      

   rospy.loginfo("Stacking items on tray")
   # stacking objects code
   print "Press key to stack objects"
   raw_input()
   #stacker.stack_objects()
   #or
   #stacker.pose_stack_objects(center_free_spot.position)
   
   rospy.loginfo("Picking up tray")
   print "Press key to pick up tray"
   raw_input()
   #tray.pick_up_tray() # ignores middle of tray, so please make sure all objects are in middle
   
   rospy.loginfo("Going to dirty table to leave tray")
   print "Press key to move"
   raw_input()
   #room3.move_to_dirty_table()
   print "Press key to put down tray"
   raw_input()
   #tray.put_down_tray() # stores tray location in tray.tray_location
   
   rospy.loginfo("PAUSE")
   rospy.sleep(10.0) 
   
   rospy.loginfo("Picking up stack of plates")
   # pickup plate stack code
   print "Press key to pick up stack"
   raw_input()
   #picker.pick_plate_stack()
   print "Press key to move"
   raw_input()
   #room3.goto_table()
   print "Press key to drop on table"
   raw_input()
   #picker.drop_stack_on_table()
   
   rospy.loginfo("Unstacking tableware")
   # unstacking plates code
   print "Press key to unstack"
   raw_input()
   #picker.put_plate_from_stack_on_table()

   # unstacking cups
   print "Press key to move"
   raw_input()
   rospy.loginfo("Picking up and placing stack of cups")
   ou.unstack_objects()
